﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Globalization;

namespace Brain
{
    public static class Vector3Util
    {
        public static bool Equal(Vector3 v1, Vector3 v2, float Epsilon)
        {
            float value = Math.Abs(v1.X - v2.X);
            value += Math.Abs(v1.Y - v2.Y);
            value += Math.Abs(v1.Z - v2.Z);
            return value < Epsilon;
        }

        /// <summary>
        /// Returns the Number of Zeros this Vector3 have
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static ushort HowManyZeroes(Vector3 v)
        {
            ushort much = 0;
            if (v.X == 0)
            {
                much++;
            }
            if (v.Y == 0)
            {
                much++;
            }
            if (v.Z == 0)
            {
                much++;
            }
            return much;
        }

        /// <summary>
        /// Returns X + Y + Z / 3
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static float Average(Vector3 vec)
        {
            return (vec.X + vec.Y + vec.Z) / 3;
        }

        /// <summary>
        /// Don't let the Vector3 pass the value
        /// </summary>
        /// <param name="vector"></param>
        /// <param name="minX"></param>
        /// <param name="maxX"></param>
        /// <param name="minY"></param>
        /// <param name="maxY"></param>
        /// <param name="minZ"></param>
        /// <param name="maxZ"></param>
        /// <returns></returns>
        public static Vector3 Clamp(Vector3 vector, float minX, float maxX,
            float minY, float maxY, float minZ, float maxZ)
        {
            return new Vector3(FloatUtil.Clamp(vector.X, minX, maxX),
                FloatUtil.Clamp(vector.Y, minY, maxY),
                FloatUtil.Clamp(vector.Z, minZ, maxZ));
        }


        /// <summary>
        /// Don't let the Vector3 pass the value
        /// </summary>
        /// <param name="vector"></param>
        /// <param name="minX"></param>
        /// <param name="maxX"></param>
        /// <param name="minY"></param>
        /// <param name="maxY"></param>
        /// <param name="minZ"></param>
        /// <param name="maxZ"></param>
        /// <returns></returns>
        public static Vector3 Clamp(Vector3 vector, Vector3 min, Vector3 max)
        {
            return new Vector3(FloatUtil.Clamp(vector.X, min.X, max.X),
                FloatUtil.Clamp(vector.Y, min.Y, max.Y),
                FloatUtil.Clamp(vector.Z, min.Z, max.Z));
        }

        public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
        {
            return new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,
                                 -(vector1.X * vector2.Z - vector2.X * vector1.Z),
                                 vector1.X * vector2.Y - vector2.X * vector1.Y);
        }

        /// <summary>
        /// Helper method computes a point on a circle. - From Geometrics Sample
        /// </summary>
        public static Vector3 GetCircleVector(int i, int tessellation)
        {
            float angle = i * MathHelper.TwoPi / tessellation;

            float dx = (float)Math.Cos(angle);
            float dz = (float)Math.Sin(angle);

            return new Vector3(dx, 0, dz);
        }
        
        /// <summary>
        /// Returns (Vec1 + Vec2) / 2
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static Vector3 Average(Vector3 vec1, Vector3 vec2)
        {
            return (vec1 + vec2) / 2;
        }
        /// <summary>
        /// Returns (vec1 + vec2 + vec3) / 3
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static Vector3 Average(Vector3 vec1, Vector3 vec2, Vector3 vec3)
        {
            return (vec1 + vec2 + vec3) / 3;
        }

        public static Vector3 NextMultiple(int X, int Y, int Z, int MultipleX, int MultipleY, int MultipleZ)
        {
            return new Vector3(MathUtil.NextMultiple(X, MultipleX),
                MathUtil.NextMultiple(Y, MultipleY),
                MathUtil.NextMultiple(Z, MultipleZ));
        }
        public static Vector3 NextMultiple(Vector3 value, int MultipleX, int MultipleY, int MultipleZ)
        {
            return NextMultiple((int)value.X, (int)value.Y, (int)value.Z,
                MultipleX, MultipleY, MultipleZ);
        }

        /// <summary>
        /// Calculates the Normalized Destination from one Vector3 to another
        /// </summary>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <returns></returns>
        public static Vector3 Direction(Vector3 origin, Vector3 destination)
        {
            Vector3 direction = destination - origin;
            if (direction != Vector3.Zero)
            {
                direction.Normalize();
            }

            return direction;
        }

        /// <summary>
        /// Returns the Absolute value of this Vector3
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static Vector3 Abs(Vector3 vec)
        {
            return new Vector3(FloatUtil.Absolute(vec.X),
                FloatUtil.Absolute(vec.Y),
                FloatUtil.Absolute(vec.Z));
        }

        /// <summary>
        /// Used for Picking
        /// </summary>
        /// <param name="unitVector3"></param>
        /// <returns></returns>
        public static Vector3 ZeroNotUnit(Vector3 unitVector3, Vector3 vec)
        {
            Vector3 newVec = vec;

            if (unitVector3.X == 0)
                newVec.X = 0;
            if (unitVector3.Y == 0)
                newVec.Y = 0;
            if (unitVector3.Z == 0)
                newVec.Z = 0;

            return newVec;
        }

        /// <summary>
        /// Converts a Rotation Vector3 from Degrees to Radians
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static Vector3 ToRadians(Vector3 vec)
        {
            return new Vector3(MathHelper.ToRadians(vec.X),
                MathHelper.ToRadians(vec.Y),
                MathHelper.ToRadians(vec.Z));
        }

        /// <summary>
        /// Linear interpolates between a value to another
        /// </summary>
        /// <param name="first"></param>
        /// <param name="second"></param>
        /// <param name="amount"></param>
        /// <returns></returns>
        public static Vector3 Lerp(Vector3 first, Vector3 second, float amount)
        {
            return new Vector3(FloatUtil.Lerp(first.X, second.X, amount),
                FloatUtil.Lerp(first.Y, second.Y, amount),
                FloatUtil.Lerp(first.Z, second.Z, amount));
        }

        /// <summary>
        /// Converts a Rotation Vector3 from Radians to Degrees
        /// </summary>
        /// <param name="vec"></param>
        /// <returns></returns>
        public static Vector3 ToDegrees(Vector3 vec)
        {
            return new Vector3(MathHelper.ToDegrees(vec.X),
                MathHelper.ToDegrees(vec.Y),
                MathHelper.ToDegrees(vec.Z));
        }

        public static List<Vector3> PointsAroundCircle(float Radius, float Y)
        {
            List<Vector3> list = new List<Vector3>();

            float radius = Radius;
            double angleStep = 15f / radius;
            Vector3 Offset = new Vector3(radius, 0, radius);

            // Middle lights
            for (double angle = 0; angle < Math.PI * 2; angle += angleStep)
            {
                int x = (int)Math.Round(radius + radius * Math.Cos(angle));
                int y = (int)Math.Round(radius + radius * Math.Sin(angle));

                Vector3 pos = new Vector3(x, Y, y);
                pos -= Offset;
                list.Add(pos);
            }

            return list;
        }

        /// <summary>
        /// Returns a Vector3 with Half values(0.5f)
        /// </summary>
        /// <returns></returns>
        public static Vector3 Half
        {
            get { return new Vector3(0.5f); }
        }

        /// <summary>
        /// Formats the Vector3 to a String. "Equivalent" to the ToString(), but gives a nicer
        /// way to show the Vector3 as a string.
        /// </summary>
        /// <param name="vec">The Vector to format</param>
        /// <returns></returns>
        public static string Format(Vector3 vec)
        {
            return "X:" + vec.X + "Y:" + vec.Y + "Z:" + vec.Z;
        }
        /// <summary>
        /// Formats the Vector3 to a String. "Equivalent" to the ToString(), but gives a nicer
        /// way to show the Vector3 as a string.
        /// </summary>
        /// <param name="vec">The Vector to format</param>
        /// <param name="X">The X representation value</param>
        /// <param name="Y">The Y representation value</param>
        /// <param name="Z">The Z representation value</param>
        /// <returns></returns>
        public static string Format(Vector3 vec, string X, string Y, string Z)
        {
            return X + vec.X + Y + vec.Y + Z + vec.Z;
        }
        /// <summary>
        /// Formats the Vector3 to a String. "Equivalent" to the ToString(), but gives a nicer
        /// way to show the Vector3 as a string.
        /// </summary>
        /// <param name="vec">The Vector to format</param>
        /// <param name="X">The X representation value</param>
        /// <param name="Y">The Y representation value</param>
        /// <param name="Z">The Z representation value</param>
        /// <param name="charOffset">Populate and add Empty Spaces, so the string won't move</param>
        /// <returns></returns>
        public static string Format(Vector3 vec, string X, string Y, string Z, int charOffset)
        {
            string vecX = vec.X.ToString();
            while (vecX.Length < charOffset)
            {
                vecX = " " + vecX;
            }
            string vecY = vec.Y.ToString();
            while (vecY.Length < charOffset)
            {
                vecY = " " + vecY;
            }
            string vecZ = vec.Z.ToString();
            while (vecZ.Length < charOffset)
            {
                vecZ = " " + vecZ;
            }
            return X + vecX + Y + vecY + Z + vecZ;
        }

        /// <summary>
        /// Returns the X, Y and Z values of a Vector4 in a Vector3.
        /// </summary>
        /// <param name="vec">The Vector4 you wnat to convert</param>
        /// <returns>A vector3 with the Vector4 values</returns>
        public static Vector3 Vector4ToVector3(Vector4 vec)
        {
            return new Vector3(vec.X, vec.Y, vec.Z);
        }

        public static float ArcTanAngle(float X, float Y)
        {
            if (X == 0)
            {
                if (Y == 1)
                    return (float)MathHelper.PiOver2;
                else
                    return (float)-MathHelper.PiOver2;
            }
            else if (X > 0)
                return (float)Math.Atan(Y / X);
            else if (X < 0)
            {
                if (Y > 0)
                    return (float)Math.Atan(Y / X) + MathHelper.Pi;
                else
                    return (float)Math.Atan(Y / X) - MathHelper.Pi;
            }
            else
                return 0;
        }

        /// <summary>
        /// Returns Euler angles that point from one point to another
        /// </summary>
        /// <param name="from"></param>
        /// <param name="location"></param>
        /// <returns></returns>
        public static Vector3 AngleTo(Vector3 from, Vector3 location)
        {
            Vector3 angle = new Vector3();
            Vector3 v3 = Vector3.Normalize(location - from);
            angle.X = (float)Math.Asin(v3.Y);
            angle.Y = ArcTanAngle(-v3.Z, -v3.X);
            return angle;
        }

        public static Vector3 NearestPoint(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
        {
            Vector3 lineDirection = Vector3.Normalize(lineEnd - lineStart);
            float closestPoint = Vector3.Dot((point - lineStart), lineDirection) / Vector3.Dot(lineDirection, lineDirection);
            return lineStart + (closestPoint * lineDirection);
        }

        public static Vector3 NearestPointStrict(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
        {
            Vector3 fullDirection = lineEnd - lineStart;
            Vector3 lineDirection = Vector3.Normalize(fullDirection);
            float closestPoint = Vector3.Dot((point - lineStart), lineDirection) / Vector3.Dot(lineDirection, lineDirection);
            return lineStart + (MathHelper.Clamp(closestPoint, 0.0f, fullDirection.Length()) * lineDirection);
        }


        // test if a Vector3 is close to another Vector3 (due to floating point inprecision)
        // compares the square of the distance to the square of the range as this 
        // avoids calculating a square root which is much slower than squaring the range
        public static bool Approx(Vector3 val, Vector3 about, float range)
        {
            return ((val - about).LengthSquared() < range * range);
        }

    }
}
